#ifndef CONFIG_H_
#define CONFIG_H_

// Site Configuration 
//--------------------------------------------------------
// Server
#define SERVER_ARDUI_FD "/dev/ttyACM0"
#define SERVER_ARDUI_BAUD 115200
#define SERVER_JOY_FD "/dev/input/js0"
#define SERVER_DELAY_US 75000
//#define SERVER_DELAY_US 500000

// Connection stuff
#define PORT_NO 5006
//#define SERVER_HOSTNAME "10.2.38.105"
#define SERVER_HOSTNAME "rascal.caltech.edu"
//#define SERVER_HOSTNAME "131.215.231.200"
//#define SERVER_HOSTNAME "192.168.1.4"
//#define SERVER_HOSTNAME "10.2.39.73"

// Rover Computer
#define ROVER_ARDUI_FD "/dev/ttyACM1"
#define ROVER_ARDUI_BAUD 115200
#define ROVER_DELAY_MS 100


// General String Options

#define STRING_DELINATOR '#'
#define STRING_END_POINT '$'
#define STRING_STAR_POINT '~'

// Command Components
//--------------------------------------------------------

#define NUM_COMMAND_COMS 28

// Enum (This has to be the shittest way ever imagined)
enum Command_Component {
  FR_D_MOTOR, // Front Right Drive Motor 0 
  FL_D_MOTOR, // Front Left Drive Motor 1
  RR_D_MOTOR, // Rear Right Drive Motor 2
  RL_D_MOTOR, // Rear Left Drive Motor 3
  FR_S_MOTOR_0, // Front Right Steer Servo Part One 4
  FR_S_MOTOR_1, // Front Right Steer Servo Part Two 5 
  FL_S_MOTOR_0,// Front Left Steer Servo Part One 6
  FL_S_MOTOR_1,// Front Left Steer Servo Part Two 7
  RR_S_MOTOR_0,// Rear Right Steer Servo Part One 8
  RR_S_MOTOR_1, // Rear Right Steer Servo Part Two 9
  RL_S_MOTOR_0, // Rear Left Steer Servo Part One 10
  RL_S_MOTOR_1, // Rear Left Steer Servo Part Two 11
  ARM_SHOUL_PAN_MOTOR_0, // Arm Shoulder Pan Servo Part One 12
  ARM_SHOUL_PAN_MOTOR_1, // Arm Shoulder Pan Servo Part Two 13
  ARM_SHOUL_ELE_MOTOR_0, // Arm SHoulder Elevation Servo Part One 14
  ARM_SHOUL_ELE_MOTOR_1, // Arm SHoulder Elevation Servo Part One 15
  ARM_ELBOW_MOTOR_0, // Arm Elbow Servo Part one 16
  ARM_ELBOW_MOTOR_1, // Arm Elbow Servo Part two 17
  ARM_WRIST_MOTOR_0, // Arm Wrist Servo Part one 18
  ARM_WRIST_MOTOR_1, // Arm Wrist Servo Part two 19
  ARM_GRIP_MOTOR_0, // Arm Gripper Servo Part one 20
  ARM_GRIP_MOTOR_1, // Arm Gripper Servo Part two 21
  ARM_TWIST_MOTOR_0, // Arm Twist Servo Part one 22
  ARM_TWIST_MOTOR_1, // Arm Twist Servo Part two 23
  MAST_ELE_MOTOR_0, // Mast Elevation Servo Part one 24 
  MAST_ELE_MOTOR_1, // Mast Elevation Servo Part two 25
  MAST_PAN_MOTOR_0, // Mast Elevation Servo Part one 26
  MAST_PAN_MOTOR_1, // Mast Elevation Servo Part two 27
};

struct CommandStateNames{
  static const char* names[];
  // Below is in config.cpp
};

// Input Components
//-------------------------------------------------------

#define NUM_INPUT_COMS 15

// Enum (This has got to be the shittest way in the world)
enum InputState_Component{
  JOY_X, // Joystick X value 0
  JOY_Y, // Joystick Y value 1
  JOY_TWI, // Joystick Twist value 2
  JOY_TH_X, // Joystick Top Hat X value 3
  JOY_TH_Y,  // Joystick Top Hat Y value 4
  JOY_BUT_0, // Joystick buttons value 5 (buttons 1 - 8)
  JOY_BUT_1, // Joystick buttons(only 4 least sig) 6 (buttons 9 - 12)
  MINI_SHOUL_PAN, // Mini arm shoulder pan 7
  MINI_SHOUL_ELE, // Mini arm shoulder elevation 8
  MINI_ELBOW, // Mini arm elbow 9
  MINI_WRIST, // Mini arm wrist 10
  MINI_GRIP, // Mini arm gripper 11
  MINI_TWIST, // Mini arm twist 12
  BUTTONS, // Misc buttons 13
  JOY_THR, // Joystick throttle 14
};

struct InputStateNames{
  static const char* names[];
  // Below is in config.cpp
  /*const char* InputStateNames::names[] = {
  "JOY_X",
  "JOY_Y",
  "JOY_TWI",
  "JOY_TH_X",
  "JOY_TH_Y",
  "JOY_BUT_0",
  "JOY_BUT_1",
  "MINI_SHOUL_PAN",
  "MINI_SHOUL_ELE",
  "MINI_ELBOW",
  "MINI_WRIST",
  "MINI_GRIP",
  "BUTTONS",
  };*/
};

// Rover State Compoentns
//-------------------------------------------------------

#define NUM_ROVER_COMS 2

// Enum (Do I have to repeat myself)
enum RoverState_Component{
  SENSOR_1, // Sensor 1 0 
  SENSOR_2, // Sensor 2 1
};

// Motor Configuration
//-------------------------------------------------------

#define NUM_MOTORS 4

// Enum (Again)
enum Motor{
  FR_D, // Front Right Drive Motor 0
  FL_D, // Front Left Drive Motor 1
  RR_D, // Rear Right Drive Motor 2
  RL_D, // Rear Left Drive Motor 3
};

struct MotorNames{
  static const char* names[];
  // Below is in config.cpp
  /*const char* MotorNames::names[] = {
  "Front Right Drive Motor", // 0
  "Front Left Drive Motor", // 1
  "Rear Right Drive Motor", // 2
  "Read Left Drive Motor", // 3
  };*/
};


// Servo Configuration
//-------------------------------------------------------

#define NUM_SERVOS 12

// Enum (I'm getting sick and tired of writing this)
enum Servos{
  FR_S, // Front Right Steering Servo 0
  FL_S, // Front Left Steering Servo 1
  RR_S, // Rear Right Steering Servo 2
  RL_S, // Rear Left Steering Servo 3
  MAST_PAN, // Mast Pan Servo 4
  MAST_TIT, // Mast Tilt Servo 5
  MAST_ELE, // Mast Elevation Servo 6
  ARM_SHOUL_PAN, // Arm Shoulder Pan Servo 7
   ARM_SHOUL_ELE, // Arm Shoulder Elevation Servo 8
  ARM_ELBOW, // Arm Elbow Servo 9
  ARM_WRIST, // Arm Wrist Servo 10
  ARM_GRIP, // Arm Gripper Servo 11
};

struct ServoNames{
  static const char* names[];
  // Below is in config.cpp
   /*const char* Servo_Names::names[] =  {
  "Front Right Steering Servo", // 0
  "Front Left Steering Servo", // 1
  "Rear Right Steering Servo", // 2
  "Rear Left Steering Servo", // 3
  "Mast Pan Servo", // 4
  "Mast Tilt Servo", // 5
  "Mast Elevation Servo", // 6
  "Arm Shoulder Pan Servo", // 7
  "Arm Shoulder Elevation Servo", // 8
  "Arm Elbow Servo", // 9
  "Arm Wrist Servo", // 10
  "Arm Grip Servo", // 11
  };*/
};

// Joystick Configuration
//-------------------------------------------------------
/*
 * Joystick all the way forward is y = 255
 * Joystick all the way right is x = 255
 * Joystick all the way clockwise is twi = 1
 */

#define JOYSTICK_MAX -32767.0

#define JOYSTICK_MID 128
#define JOYSTICK_MAX_MOD_X 128
#define JOYSTICK_MAX_MOD_Y 128

#define JOYSTICK_X_AXIS 0
#define JOYSTICK_Y_AXIS 1
#define JOYSTICK_TWI_AXIS 2
#define JOYSTICK_THR_AXIS 3
#define JOYSTICK_TH_X_AXIS 4
#define JOYSTICK_TH_Y_AXIS 5

#define JOYSTICK_BUTTON_1 0
#define JOYSTICK_BUTTON_2 1
#define JOYSTICK_BUTTON_3 2
#define JOYSTICK_BUTTON_4 3
#define JOYSTICK_BUTTON_5 4
#define JOYSTICK_BUTTON_6 5
#define JOYSTICK_BUTTON_7 6
#define JOYSTICK_BUTTON_8 7

#define JOYSTICK_BUTTON_9 8
#define JOYSTICK_BUTTON_10 9
#define JOYSTICK_BUTTON_11 10
#define JOYSTICK_BUTTON_12 11

#define JOYSTICK_BUTTON_1_MASK 1 //0b 0000 0001
#define JOYSTICK_BUTTON_2_MASK 2 //0b 0000 0010
#define JOYSTICK_BUTTON_3_MASK 4 //0b 0000 0100
#define JOYSTICK_BUTTON_4_MASK 8 //0b 0000 1000
#define JOYSTICK_BUTTON_5_MASK 16 //0b 0001 0000
#define JOYSTICK_BUTTON_6_MASK 32 //0b 0010 0000
#define JOYSTICK_BUTTON_7_MASK 64 //0b 0100 0000
#define JOYSTICK_BUTTON_8_MASK 128 //0b 1000 0000

// Stored on the second button value
#define JOYSTICK_BUTTON_9_MASK 1 //0b 0000 0001
#define JOYSTICK_BUTTON_10_MASK 2 //0b 0000 0010
#define JOYSTICK_BUTTON_11_MASK 4 //0b 0000 0100
#define JOYSTICK_BUTTON_12_MASK 8 //0b 0000 1000

// Rover Real CONFIGURATION
//-------------------------------------------------------

#define FR_WHEEL_X 15.7
#define FR_WHEEL_Y 12.9

#define FL_WHEEL_X -15.7
#define FL_WHEEL_Y 12.9

#define RR_WHEEL_X 15.7
#define RR_WHEEL_Y -12.9

#define RL_WHEEL_X -15.7
#define RL_WHEEL_Y -12.9

// Command Configuration
//-------------------------------------------------------

#define JOY_X_DEADBAND 5
#define JOY_Y_DEADBAND 5
#define JOY_TWI_DEADBAND 5

#define ENABLE_TWIST_BUT JOYSTICK_BUTTON_2
#define ENABLE_TWIST_MASK JOYSTICK_BUTTON_2_MASK

// Also sorta in rover-ar-cfg/servo_config.h
#define SERVO_MS_0 1800
#define SERVO_MS_180 1200
#define SERVO_MS_MIN 500
#define SERVO_MS_MAX 2500

#define SERVO_STRAIGHT_0 249
#define SERVO_STRAIGHT_1 1

#define SERVO_MS_DIFF abs(SERVO_MS_0 - SERVO_MS_180)

#define MAX_VALUE_SPLIT 255
#define MIN_VALUE_SPLIT 1

#define WORST_X FR_WHEEL_X
#define WORST_Y FR_WHEEL_Y

#define MAX_MOTOR_ROTATE 127
#define MAX_MOTOR_SPEED 127
#define MOTOR_MIDPOINT 128

#define MOTOR_MAX_PWM 255
#define MOTOR_MIN_PWM 1

#define SERVO_SLEW_RATE 150./1100.

// Turbo Mode
#define FR_TURBO_BUTTON JOYSTICK_BUTTON_10_MASK
#define FL_TURBO_BUTTON JOYSTICK_BUTTON_9_MASK
#define RR_TURBO_BUTTON JOYSTICK_BUTTON_12_MASK
#define RL_TURBO_BUTTON JOYSTICK_BUTTON_11_MASK

#define TURBO_GAIN 1.75

#endif
